#!/usr/bin/env python3

import sys
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from PyQt5.QtWidgets import *
from PyQt5.QtCore import *
from general_interface.action import SwitchConnection


class ROSNode(Node):
    def __init__(self):
        super().__init__("connection_ui")
        self.action_client = ActionClient(self, SwitchConnection, "switch_connection")

    def send_goal(self, goal, feedback_callback=None):
        goal_msg = SwitchConnection.Goal()
        goal_msg.command = goal
        goal_msg.timeout = 10.0
        if not self.action_client.wait_for_server(timeout_sec=0.1):
            self.get_logger().error("find connection action server failed")
            return None
        return self.action_client.send_goal_async(
            goal_msg, feedback_callback=feedback_callback
        )


class View(QWidget):
    def __init__(self):
        super().__init__()
        self.setWindowTitle("Connection UI")
        self.resize(400, 300)
        self.initUI()
        rclpy.init()
        self.node = ROSNode()
        self.timer = QTimer()
        self.timer.timeout.connect(self.on_ros_tick)
        self.timer.start(100)

    def closeEvent(self, event):
        self.timer.stop()
        self.rclpy_shutdown()
        event.accept()

    def on_ros_tick(self):
        rclpy.spin_once(self.node, timeout_sec=0.1)

    def initUI(self):
        self.layout = QVBoxLayout()
        self.setLayout(self.layout)

        self.buttonC = QPushButton("Connect")
        self.layout.addWidget(self.buttonC)

        self.buttonD = QPushButton("Disconnect")
        self.layout.addWidget(self.buttonD)

        self.buttonP = QPushButton("Pause")
        self.layout.addWidget(self.buttonP)

        self.lable = QLabel("Status:")
        self.lable.setFixedHeight(40)
        self.layout.addWidget(self.lable)

        self.buttonC.clicked.connect(self.on_click_connect)
        self.buttonD.clicked.connect(self.on_click_disconnect)
        self.buttonP.clicked.connect(self.on_click_pause)

    def on_click_connect(self):
        self.future = self.node.send_goal(
            SwitchConnection.Goal.CONNECT, feedback_callback=self.on_feedback
        )
        if self.future == None:
            self.lable.setText("No server find")
            return
        self.future.add_done_callback(self.on_response)

    def on_click_disconnect(self):
        self.future = self.node.send_goal(
            SwitchConnection.Goal.DISCONNECT, feedback_callback=self.on_feedback
        )
        if self.future == None:
            self.lable.setText("No server find")
            return
        self.future.add_done_callback(self.on_response)

    def on_click_pause(self):
        self.future = self.node.send_goal(
            SwitchConnection.Goal.STOP, feedback_callback=self.on_feedback
        )
        if self.future == None:
            self.lable.setText("No server find")
            return
        self.future.add_done_callback(self.on_response)

    def on_response(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.lable.setText("Goal rejected")
            return
        self.lable.setText("Goal accepted")
        self.result_future = goal_handle.get_result_async()
        self.result_future.add_done_callback(self.on_result)

    def on_feedback(self, feedback):
        current = feedback.current
        if current == SwitchConnection.Feedback.CONNECTING:
            self.lable.setText("Feedback:Connecting...")
        elif current == SwitchConnection.Feedback.DISCONNECTING:
            self.lable.setText("Feedback:Disconnecting...")
        elif current == SwitchConnection.Feedback.PAUSING:
            self.lable.setText("Feedback:Pausing...")
        else:
            self.lable.setText(f"Feedback:Unknown feedback,current={current}")

    def on_result(self, future):
        result = future.result().result
        if result.result == SwitchConnection.Result.PAUSED:
            self.lable.setText("Result:Paused")
        elif result.result == SwitchConnection.Result.CONNECTED:
            self.lable.setText("Result:Connected")
        elif result.result == SwitchConnection.Result.DISCONNECTED:
            self.lable.setText("Result:Disconnected")
        else:
            self.lable.setText(f"Result:Unknown result,code={result.result}")


if __name__ == "__main__":
    app = QApplication(sys.argv)
    window = View()
    window.show()
    app.exec_()
